/*
 Copyright 2013--Present JMM_PROGNAME
 
 This file is distributed under the terms of the JMM_PROGNAME License.
 
 You should have received a copy of the JMM_PROGNAME License.
 If not, see <JMM_PROGNAME WEBSITE>.
*/
// CREATED    : 9/23/2015
// LAST UPDATE: 9/23/2015


// STL
#include <cmath>      // std::abs()
#include <functional> // std::function<>
#include <tuple>      // std::tuple<>, std::make_tuple(), std::tie()

// jScience
#include "jScience/linalg/Matrix.hpp" // Matrix<>
#include "jScience/linalg/Vector.hpp" // Vector<>

// stats++
#include "statsxx/optimization/line_search_methods/LineSearchMethod.hpp" // LineSearchMethod


//========================================================================
//========================================================================
//
// NAME: std::tuple<
//                  int,
//                  Vector<double>,
//                  double
//                  > Powell(
//                           std::function<double(const Vector<double> &)> f,
//                           Vector<double> p,
//                           const Matrix<double> &Xi,
//                           const int max_it,
//                           const double ftol
//                           )
//
// DESC: Direction set (Powell's) method in multidimensions.
//
// INPUT:
//     std::function<double(const Vector<double> &)> f : function to be minimized
//     Vector<double> p  : initial starting point
//     Matrix<double> X  : initial set of directions (each column of X is a direction vector)
//
// OUTPUT:
//     int info          : return info (0 == success, 1 == too many iterations, -1 == line minimization error)
//     Vector<double> p  : minimum point
//     double fret       : f() at the minimum
//
//========================================================================
//========================================================================
inline std::tuple<
                  int,
                  Vector<double>,
                  double
                  > Powell(
                           std::function<double(const Vector<double> &)> f,
                           Vector<double> p,
                           Matrix<double> X,
                           const int max_it,
                           const double ftol
                           )
{
//    int info;
    double fret;
//    Vector<double> p;
    
    LineSearchMethod lsm;
    
    // initialize
    Vector<double> pt = p;
    
    fret = f(p);
    
    // iterate
    for(int iter = 0; ; ++iter)
    {
        double fp = fret;
        
        // in each iteration: loop over all directions, minimize (along them), and record if it was the largest decrease so far
        decltype(X.size(1)) jbig = 0;
        double del = 0.0;

        for(auto j = 0; j < X.size(1); ++j)
        {
            Vector<double> x = X.column(j);
            
            double fptt = fret;

            int lm_info;
            std::tie(lm_info, p, fret) = lsm.line_minimize(f, p, x);
            
            if(lm_info != 0)
            {
                return std::make_tuple(-1, p, fret);
            }
            
            if( (fptt - fret) > del )
            {
                del = fptt - fret;
                jbig = j;
            }
        }
        
//        std::cout << "fret: " << fret << '\n';
        
        // termination criterion:
        if( 2.0*(fp - fret) <= ftol*(std::abs(fp) + std::abs(fret)) )
        {
            return std::make_tuple(0, p, fret);
        }
        
        // note: wait until here to check the number of iterations, su that we can attempt one last line minimization
        if(iter == max_it)
        {
            return std::make_tuple(1, p, fret);
        }
        
        // construct the extrapolated point and the average direction moved; also save the old starting point
        Vector<double> ptt = 2.0*p - pt;
        pt = p;
        
        double fptt = f(ptt);
        
        if(fptt < fp)
        {
            double t = 2.0*(fp - 2.0*fret + fptt)*(fp - fret - del)*(fp - fret - del) - del*(fp - fptt)*(fp - fptt);
            
            if(t < 0.0)
            {
                // construct the average direction moved; move to the minimum of the this direction; save it
                Vector<double> x = p - pt;
                
                int lm_info;
                std::tie(lm_info, p, fret) = lsm.line_minimize(f, p, x);
                
                if(lm_info != 0)
                {
                    return std::make_tuple(-1, p, fret);
                }
                
                for(auto i = 0; i < X.size(0); ++i)
                {
                    X(i,jbig) = X(i,(X.size(1)-1));
                    X(i,(X.size(1)-1)) = x(i);
                }
            }
        }
    }
    
    // note: the above continuously loops until either convergence or max iterations; hence, we should never reach here 
}
